/*
 * Copyright (c) 2019, RT-Thread Development Team
 *
 * SPDX-License-Identifier: Apache-2.0
 *
 * Change Logs:
 * Date           Author       Notes
 * 2019-07-17     Wu Han       The first version
 * 2023-08-31     WangShun     adapt YoC
 */

#include "wheel.h"

wheel_t wheel_create(motor_t w_motor, encoder_t w_encoder, controller_t w_controller, float radius, unsigned short gear_ratio)
{
    //radius must not be zero, protect:
    if(radius == 0.0f) 
    {
        return NULL;
    }

    // 1. Malloc memory for wheel
    wheel_t new_wheel = (wheel_t)malloc(sizeof(struct wheel));
    if(new_wheel == NULL)
    {
        aos_cli_printf("Falied to allocate memory for new wheel");
        return NULL;
    }

    // 2. Initialize wheel
    new_wheel -> w_motor      = w_motor;
    new_wheel -> w_encoder    = w_encoder;
    new_wheel -> w_controller = w_controller;
    new_wheel -> radius       = radius;
    new_wheel -> gear_ratio   = gear_ratio;

    // 3. pre compute the speed to rpm transform ;)
    new_wheel -> speed_to_rpm = 1.0f / (radius * 2.0f * 60.0f * PI);

    return new_wheel;
}

signed long wheel_destroy(wheel_t whl)
{
    CHECK_RET_WITH_RET(whl, -1);

    aos_cli_printf("Free wheel");

    motor_destroy(whl->w_motor);
    encoder_destroy(whl->w_encoder);
    controller_destroy(whl->w_controller);

    free(whl);

    return 0;
}

signed long wheel_enable(wheel_t whl)
{
    CHECK_RET_WITH_RET(whl, -1);

    aos_cli_printf("Enabling wheel");

    // Enable PWM for motor
    motor_enable(whl->w_motor);

    // Enable Encoder's interrupt
    encoder_enable(whl->w_encoder);

    // Enable control
    controller_enable(whl->w_controller);

    return 0;
}

signed long wheel_disable(wheel_t whl)
{
    CHECK_RET_WITH_RET(whl, -1);

    aos_cli_printf("Disabling wheel");

    // Disable PWM for motor
    motor_disable(whl->w_motor);

    // Disable Encoder's interrupt
    encoder_disable(whl->w_encoder);
    
    // Disable control
    controller_disable(whl->w_controller);

    return 0;
}

signed long wheel_reset(wheel_t whl)
{
    CHECK_RET_WITH_RET(whl, -1);

    // Reset Controller
    controller_reset(whl->w_controller);

    // Reset Motor
    motor_reset(whl->w_motor);
    
    // Reset Encoder
    encoder_reset(whl->w_encoder);

    return 0;
}

/** speed = rpm x 60 x 2 x PI x radius **/
/** so : rpm = speed x 1.0f / (radius x 60 x 2 x PT) */
/** then : rpm = speed * speed_to_rpm_transform --> precomputed */
signed long wheel_set_speed(wheel_t whl, float speed)
{
    CHECK_RET_WITH_RET(whl, -1);

    return wheel_set_rpm(whl, (signed short) (speed * whl->speed_to_rpm));
}

signed long wheel_set_rpm(wheel_t whl, signed short rpm)
{
    CHECK_RET_WITH_RET(whl, -1);

    controller_set_target(whl->w_controller, rpm);
    if(whl->w_controller->target == rpm)
    {
        return 0;
    }
    else
    {
        return 1;
    }
}

void wheel_update(wheel_t whl)
{
    ROBOT_CHECK_RET_WITH_RET(whl, -1);

    // Get current rpm
    whl->rpm = encoder_measure_rpm(whl->w_encoder);

    // Automatic control calculation
    controller_update(whl->w_controller, whl->rpm);

    // Set speed
    motor_run(whl->w_motor, whl->w_controller->output);
}

void wheel_stop(wheel_t whl)
{
    ROBOT_CHECK_RET_WITH_RET(whl, -1);

    motor_stop(whl->w_motor);
}
